#include <string.h>
#include <stdio.h>
#include <stdint.h>
#include <boarddef.h>
#include "common/common.h"
#include "drv/sonar_ks103.h"
#include "drv/led.h"



uint16_t sonar_ks103_data=0;
sonar_ks103_channel_t g_sonar[CONFIG_SONAR_KS103_CHANNEL_NUM];
static uint8_t g_sonar_ch = 0;  /**< Active sonar channel index. */
//void IIC_Init(void)
//{
//RCC->APB2ENR|=1<<4;//ÏÈÊ¹ÄÜÍâÉè IO PORTC Ê±ÖÓ
//GPIOC->CRH&=0XFFF00FFF;//PC11/12 ÍÆÍìÊä³ö
//GPIOC->CRH|=0X00033000;
//GPIOC->ODR|=3<<11; //PC11,12 Êä³ö¸ß
//}
//²úÉú IIC ÆðÊ¼ÐÅºÅ
void IIC_Start(void)
{
    SDA_OUT(); //sda ÏßÊä³ö
    IIC_SDA=1;
    IIC_SCL=1;
    _delay_us(10);
    IIC_SDA=0;//START:when CLK is high,DATA change form high to low
    _delay_us(10);
    IIC_SCL=0;//Ç¯×¡ I2C ×ÜÏß£¬×¼±¸·¢ËÍ»ò½ÓÊÕÊý¾Ý
}
//²úÉú IIC Í£Ö¹ÐÅºÅ
void IIC_Stop(void)
{
     SDA_OUT();//sda ÏßÊä³ö
    IIC_SCL=0;
    IIC_SDA=0;//STOP:when CLK is high DATA change form low to high
    _delay_us(10);
    IIC_SCL=1;
    IIC_SDA=1;//·¢ËÍ I2C ×ÜÏß½áÊøÐÅºÅ
    _delay_us(10);
}
//µÈ´ýÓ¦´ðÐÅºÅµ½À´
//·µ»ØÖµ£º 1£¬½ÓÊÕÓ¦´ðÊ§°Ü
// 0£¬½ÓÊÕÓ¦´ð³É¹¦
uint8_t IIC_Wait_Ack(void)
{
    uint8_t ucErrTime=0;
    SDA_IN(); //SDA ÉèÖÃÎªÊäÈë
    IIC_SDA=1;
		_delay_us(6);
    IIC_SCL=1;
		_delay_us(6);
    while(READ_SDA)
    {
        ucErrTime++;
        if(ucErrTime>250)
        {
            IIC_Stop();
            return 1;
        }
    }
    IIC_SCL=0;//Ê±ÖÓÊä³ö 0
    return 0;
}
//²úÉú ACK Ó¦´ð
void IIC_Ack(void)
{
     IIC_SCL=0;
    SDA_OUT();
    IIC_SDA=0;
    _delay_us(10);
    IIC_SCL=1;
    _delay_us(10);
    IIC_SCL=0;
}
//²»²úÉú ACK Ó¦´ð
void IIC_NAck(void)
{
     IIC_SCL=0;
    SDA_OUT();
    IIC_SDA=1;
    _delay_us(10);
    IIC_SCL=1;
    _delay_us(10);
    IIC_SCL=0;
}
//IIC ·¢ËÍÒ»¸ö×Ö½Ú
//·µ»Ø´Ó»úÓÐÎÞÓ¦´ð
//1£¬ÓÐÓ¦´ð
//0£¬ÎÞÓ¦´ðwww.dauxi.com

void IIC_Send_Byte(uint8_t txd)
{
     uint8_t t;
    SDA_OUT();
    IIC_SCL=0;//À­µÍÊ±ÖÓ¿ªÊ¼Êý¾Ý´«Êä
    for(t=0;t<8;t++)
    {
        IIC_SDA=(txd&0x80)>>7;
        txd<<=1;
        _delay_us(10);
        IIC_SCL=1;
        _delay_us(10);
        IIC_SCL=0;
        _delay_us(10);
    }
}
//¶Á 1 ¸ö×Ö½Ú£¬ ack=1 Ê±£¬·¢ËÍ ACK£¬ ack=0£¬·¢ËÍ nACK
uint8_t IIC_Read_Byte(unsigned char ack)
{
    unsigned char i,receive=0;
    SDA_IN();//SDA ÉèÖÃÎªÊäÈë
    for(i=0;i<8;i++ )
    {
    IIC_SCL=0;
    _delay_us(10);
    IIC_SCL=1;
    receive<<=1;
    if(READ_SDA)receive++;
    _delay_us(5);
    }
    if (!ack)
    IIC_NAck();//·¢ËÍ nACK
    else
    IIC_Ack(); //·¢ËÍ ACK
    return receive;
}

uint8_t KS103_ReadOneByte(uint8_t address, uint8_t reg)
{
   uint8_t temp=0;
     IIC_Start();
    IIC_Send_Byte(address); //·¢ËÍµÍµØÖ·
    IIC_Wait_Ack();
    IIC_Send_Byte(reg); //·¢ËÍµÍµØÖ·
    IIC_Wait_Ack();
    IIC_Start();
    IIC_Send_Byte(address + 1); //½øÈë½ÓÊÕÄ£Ê½
    IIC_Wait_Ack();
    _delay_us(50); //Ôö¼Ó´Ë´úÂëÍ¨ÐÅ³É¹¦£¡£¡£¡
    temp=IIC_Read_Byte(0); //¶Á¼Ä´æÆ÷ 3
    IIC_Stop();//²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
    return temp;
}
void KS103_WriteOneByte(uint8_t address,uint8_t reg,uint8_t command)
{
     IIC_Start();
    IIC_Send_Byte(address); //·¢ËÍÐ´ÃüÁî
    IIC_Wait_Ack();
    IIC_Send_Byte(reg);//·¢ËÍ¸ßµØÖ·
    IIC_Wait_Ack();
    IIC_Send_Byte(command); //·¢ËÍµÍµØÖ·
    IIC_Wait_Ack();
    IIC_Stop();//²úÉúÒ»¸öÍ£Ö¹Ìõ¼þ
}

void change_i2c_address(unsigned char addr_old, unsigned char addr_new)
{ 
    _delay_ms(2000); // Protect the eeprom ,you can delete this sentence
    KS103_WriteOneByte(addr_old,2,0x9a);
    _delay_ms(1);
    KS103_WriteOneByte(addr_old,2,0x92);
    _delay_ms(1);
    KS103_WriteOneByte(addr_old,2,0x9e);
    _delay_ms(1);
    KS103_WriteOneByte(addr_old,2, addr_new);
    _delay_ms(500); //Protect the eeprom, you can delete this sentence
}

static uint8_t addr_map[CONFIG_SONAR_KS103_CHANNEL_NUM] = {0xd2, 0xd4, 0xd6, 0xd8, 0xda};//安装位置顺序，从左到右
// static uint8_t addr_map[CONFIG_SONAR_KS103_CHANNEL_NUM] = {0xd0, 0xd0, 0xd0, 0xd0, 0xd0};//安装位置顺序，从左到右

uint16_t sonar_ks103_get(uint8_t ch)
{
    if (ch >= CONFIG_SONAR_KS103_CHANNEL_NUM) {
        return 0;
    }

    if (g_sonar[ch].distance > SONAR_LIMIT) {
        return 0;
    }
    // sonar_distance(ch);
    return g_sonar[ch].distance;
}

static _u32 speedctl_frequency = 0;
void ks103_heartbeat(void)
{
    _u32 currentTs = getms();
    _u32 delta = currentTs - speedctl_frequency;

    if (delta >= 1) {
        speedctl_frequency = currentTs;
        // DBG_OUT("g_sonar_ch =  %d\r\n", g_sonar_ch);
        uint8_t ucI2cAddr;
        switch (g_sonar[g_sonar_ch].state) {
        case SONAR_KS103_INIT:
            ucI2cAddr = addr_map[g_sonar_ch];
            KS103_WriteOneByte(ucI2cAddr, 0X02, 0XB0);
            // sonar_trigger(g_sonar_ch);
            g_sonar[g_sonar_ch].state++;    /* Move to next state. */
            g_sonar[g_sonar_ch].ticks = getms();
            // DBG_OUT("SONAR_KS103_INIT %d\r\n", g_sonar[g_sonar_ch].ticks);
            // DBG_OUT("ucI2cAddr %d\r\n", ucI2cAddr);
            break;
        case SONAR_KS103_MEASURE: /* Wait until measurement done. */        
            if (getms() - g_sonar[g_sonar_ch].ticks > CONFIG_SONAR_KS103_TIMEOUT_MS) {
                // DBG_OUT("SONAR_KS103_MEASURE %d\r\n", getms());
                /* Timeout. Abort measurement and move to next channel. */
                uint16_t range = 0;
                ucI2cAddr = addr_map[g_sonar_ch];
                range = KS103_ReadOneByte(ucI2cAddr, 0x02);
                range <<= 8;
                range += KS103_ReadOneByte(ucI2cAddr, 0x03);
                g_sonar[g_sonar_ch].distance = (uint16_t)range;
                
                // DBG_OUT("SONAR_KS103_MEASURE ucI2cAddr %d\r\n", ucI2cAddr);
                // DBG_OUT("range =  %d\r\n", range);
                
                g_sonar[g_sonar_ch].ticks = getms();
                g_sonar[g_sonar_ch].state = SONAR_KS103_IDLE;
                g_sonar_ch++;
                if (g_sonar_ch >= CONFIG_SONAR_KS103_CHANNEL_NUM) {
                    g_sonar_ch = 0;
                }
            }
            break;
        default:
            g_sonar[g_sonar_ch].state = SONAR_KS103_INIT;
            break;
        }
    }
    
}

/**
 @brief sonar ks103 initialize.
 @param none.
 @return none.
 */
void drv_sonar_ks103_init(void)
{
    pinMode(SONAR_KS103_PORT, SONAR_KS103_SCL_PIN,   GPIO_Mode_Out_PP, GPIO_Speed_50MHz);
    pinMode(SONAR_KS103_PORT, SONAR_KS103_SDA_PIN,   GPIO_Mode_Out_PP, GPIO_Speed_50MHz);

//   PIN_SET(SONAR_KS103_PORT, SONAR_KS103_SCL_PIN, 1);
//   PIN_SET(SONAR_KS103_PORT, SONAR_KS103_SDA_PIN, 1);
//    GPIO_SetBits(SONAR_KS103_PORT,SONAR_KS103_SCL_PIN);
//    GPIO_SetBits(SONAR_KS103_PORT,SONAR_KS103_SDA_PIN);
		SDA_OUT();     //sdaÏßÊä³ö
	  IIC_SDA=1;	  	  
	  IIC_SCL=1;

      memset(g_sonar, 0, sizeof(g_sonar));      
}